﻿#pragma execution_character_set("utf-8")
/*
	btosgVehicle.h
	Miguel Leitao, 2016
*/

#ifndef BTOSGVEHICLE_H
#define BTOSGVEHICLE_H 1

#include <osg/Material>

#include <btBulletCollisionCommon.h>
#include <btBulletDynamicsCommon.h>
#include <BulletDynamics/Vehicle/btRaycastVehicle.h>

#include "btosg.h"

#include <osg/Material>
#include <osg/Texture2D>

/*
class btosgWheel : public btosgCylinder {
    public:
        btosgWheel(btVector3 pos, double ang) : btosgCylinder(0.4, 0.2) {
            setPosition(pos);
            setRotation(osg::Quat(ang,osg::Vec3(1.,0.,0.)));
            setTexture("wheel.png");
            body->setFriction(100.);
        }
        virtual void update() {
        if (body) {
            btTransform wTrans;
            body->getMotionState()->getWorldTransform(wTrans);

            if ( model ) {
                model->setAttitude(bt2osg_Quat(wTrans.getRotation()));
                model->setPosition(bt2osg_Vec3(wTrans.getOrigin()));
            }
            //logPosition();

        }
    }
};
*/

/// A four-wheeled vehicle based on btRaycastVehicle.
class btosgVehicle: public btosgObject {
private:
    btDefaultVehicleRaycaster *rayCaster;
public:
    float dx;			///< Vehicle x dimension.
    float dy;			///< Vehicle y dimension.
    float dz;			///< Vehicle < dimension.
    btosgVec3 dim;		///< Vehicle's dimensions in world coordinates.
    btosgVec3 up;		///< Vehicle's up vector. Usually, up=-gravity
    btosgVec3 front;	///< Vehicle's up vector.
    btRaycastVehicle *vehicle;	///< A btRaycastVehicle object.
    osg::Node* pNode; ///own node
    osg::ref_ptr<osg::PositionAttitudeTransform> wheel[4]; 	///< Transformation from vehicle's referential to wheel position.
    double wheelRotation[4];				///< Wheels' rotation angles.
    btosgVehicle(btosgWorld *world, btosgVec3 dimLocal = btosgVec3(2.,0.4,4.), double m=1000. ) {
        /// btosgVehicle constructor.
        btVector3 grav = world->dynamic->getGravity();
        int grav_axis = grav.minAxis();
        switch ( grav_axis ) {
        case 0:
            fprintf(stderr,"Gravity direction %d (%f,%f,%f) not supported\n",grav_axis,grav[0],grav[1],grav[2]);
            break;
        case 1:
            up = btosgVec3(0., 1., 0.);
            front = btosgVec3(0., 0., 1.);
            break;
        case 2:
            up = btosgVec3(0., 0., 1.);
            front = btosgVec3(0., 1., 0.);
            break;
        }
        // dimLocal contains (Width,Height,Lenght)
        dim[0] = dimLocal[0];
        dim[1] = dimLocal[1]*up[1] + dimLocal[2]*front[1];
        dim[2] = dimLocal[2]*front[2] + dimLocal[1]*up[2];
        // dim contains (dx,dy,dz)
        dx = dim[0];
        dy = dim[1];
        dz = dim[2];
        osg::ref_ptr<osg::Geode> geo = new osg::Geode();
        if ( geo.valid() ) {
            // Box to visualize the chassis
            osg::ref_ptr<osg::Shape> sp = new osg::Box( osg::Vec3(0.,0.,0.), dx, dy, dz);
            if ( sp) {
                osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(sp);
                if ( sd )
                    geo->addDrawable(sd);
                else fprintf(stderr,"Error creating osg::Shape\n");
            } else fprintf(stderr,"Error creating osg::Shape\n");
        } else fprintf(stderr,"Error creating Geode\n");
        if ( !model )	model = new osg::PositionAttitudeTransform;
        model->addChild(geo);
        //表示阴影投影对象
        model->setNodeMask(CastsShadowTraversalMask);

        pNode = geo;

        osg::ref_ptr<osg::Material> mat = new osg::Material;
        //设置环境光反射
        mat->setAmbient (osg::Material::FRONT_AND_BACK, osg::Vec4(0.4, 0.3, 0., 1.0));
        //设置漫反射
//        mat->setDiffuse (osg::Material::FRONT_AND_BACK, osg::Vec4(0.8, 0.7, 0.0, 1.0));
        mat->setDiffuse (osg::Material::FRONT_AND_BACK, osg::Vec4(0.8, 0.7, 1.0, 1.0));
        //设置镜面反射
        mat->setSpecular(osg::Material::FRONT_AND_BACK, osg::Vec4(0.1, 0.1, 0, 1.0));
        //设置物体的光泽度
        mat->setShininess(osg::Material::FRONT_AND_BACK, 12);

        setMaterial(mat);

        setMass(m);
        // Center-of-gravity is shifted by shift ????
        btTransform shift(btQuaternion::getIdentity(), btVector3(0.f, 0.f, 0.f));
        // Box for collisions and center-of-gravity definition
        btCollisionShape* boxShape = new btBoxShape(btVector3(dim)/2.);
        //btCompoundShape是Bullet Physics库中的一个类，用于创建复合形状，即由多个子形状组合而成的形状。
        btCompoundShape* chassisShape = new btCompoundShape();
        chassisShape->addChildShape(shift, boxShape);
        shape = chassisShape;
        if ( !shape ) fprintf(stderr,"Error creating btShape\n");

        createRigidBody();
        //btDefaultVehicleRaycaster：默认车辆射线投射器。
//        btRaycastVehicle：射线投射车辆。
//        btVehicleTuning：车辆调整参数。

        //射线投射车辆 由调整参数、 射线投射器和刚体组成

        rayCaster = new btDefaultVehicleRaycaster(world->dynamic);
        btRaycastVehicle::btVehicleTuning tuning;
        vehicle = new btRaycastVehicle(tuning, body, rayCaster);

        body->setActivationState(DISABLE_DEACTIVATION);
        world->dynamic->addVehicle(this->vehicle);

        //Adds the wheels to the vehicle
        btVector3 halfExtents = btVector3(dim[0]/2.,dim[1]/2.,dim[2]/2.);
        addWheels(&halfExtents, this->vehicle, tuning);


        printf("vehicle body created\n");

        //设置协调系统
        vehicle->setCoordinateSystem( 0, btVector3(up).maxAxis(), btVector3(front).maxAxis() );
        return;
    }

    void addWheels(
        btVector3* halfExtents,
        btRaycastVehicle* vehicle,
        btRaycastVehicle::btVehicleTuning tuning)
    {
        //投射方向，转轴方向，轮胎的悬挂长度，宽度，半径，连接的高度，轴的位置 重点
        /// Adds four wheels to the vehicle.

        // The direction of the raycast, the btRaycastVehicle uses raycasts
        // to sense the ground under the wheels.
        //确定轮子投射的方向
        btVector3 wheelDirectionCS0(-btVector3(up));

        // The axis which the wheel rotates arround
//        定义了绕其旋转的轴^表示向量的叉乘操作
        btVector3 wheelAxleCS( btosgVec3(front ^ up));

        // center-of mass height if mass=0
        // height = suspensionRestLength-mass.g/m_suspensionStiffness
        //定义了悬挂系统的初始长度
        btScalar suspensionRestLength(0.80);
        //车轮的宽度
        btScalar wheelWidth(0.2);
        //车轮的半径
        btScalar wheelRadius(0.3);
        // The height where the wheels are connected to the chassis
        //
//        double connectionHeight = 0.4;
        //连接点相对于车辆的高度位置
        double connectionHeight = 0.4;
        //前轴位置
        double frontAxisPos = 1.25;
        //后轴位置
        double backAxisPos = 1.25;

        // All the wheel configuration assumes the vehicle is centered at the origin and a right handed coordinate system is used
        btVector3 wheelConnectPoint;

        // Adds the front wheels
        //重点，这里我也看的不是特别懂
        wheelConnectPoint = btVector3(  halfExtents->x() + wheelWidth/2.,
                                        connectionHeight*up[1] + frontAxisPos*front[1],
                                        connectionHeight*up[2] + frontAxisPos*front[2]);

        vehicle->addWheel(wheelConnectPoint, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, true);
        vehicle->addWheel(wheelConnectPoint*btVector3(-1, 1, 1), wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, true);
        //重点
        //Adds the rear wheels
        wheelConnectPoint = btVector3(  halfExtents->x() + wheelWidth/2.,
                                        connectionHeight*up[1] - backAxisPos*front[1],
                                        connectionHeight*up[2] - backAxisPos*front[2]);
        vehicle->addWheel(wheelConnectPoint, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, false);
        vehicle->addWheel(wheelConnectPoint*btVector3(-1, 1, 1), wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, false);

        // Create one graphics wheel
        osg::ref_ptr<osg::Geode> geo = new osg::Geode;
        osg::ref_ptr<osg::Shape> sp;
        sp = new osg::Cylinder( osg::Vec3(0.,0.,0.), wheelRadius, wheelWidth);
        if ( sp) {
            osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(sp);
            if ( sd ) {
                geo->addDrawable(sd);
                // Setup tire material

                osg::ref_ptr<osg::StateSet> stateset = new osg::StateSet();
                osg::ref_ptr<osg::Material> mat = new osg::Material;
                //设置环境环境光反射属性
                mat->setAmbient (osg::Material::FRONT_AND_BACK, osg::Vec4(0., 0., 0., 1.0));
                //设置漫反射
                mat->setDiffuse (osg::Material::FRONT_AND_BACK, osg::Vec4(0.1, 0.1, 0.1, 1.0));
                //设置镜面反射
                mat->setSpecular(osg::Material::FRONT_AND_BACK, osg::Vec4(0, 0, 0, 1.0));
                //设置光泽度属性
                mat->setShininess(osg::Material::FRONT_AND_BACK, 64);
                //设置属性
                stateset->setAttribute(mat, osg::StateAttribute::OVERRIDE|osg::StateAttribute::ON);

                stateset->setTextureAttributeAndModes(0,NULL, osg::StateAttribute::OFF);
                sd->setStateSet( stateset );
            }   else fprintf(stderr,"Error creating osg::Shape\n");
        } else fprintf(stderr,"Error creating osg::Shape\n");

        sp = new osg::Cylinder( osg::Vec3(0.,0.,0.), wheelRadius*0.75, wheelWidth+0.005);
        if ( sp) {
            osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(sp);
            if ( sd ) {
                geo->addDrawable(sd);
                osg::ref_ptr<osg::StateSet> stateset = new osg::StateSet();
                // Wheel material
                osg::ref_ptr<osg::Material> mat = new osg::Material;
                mat->setAmbient (osg::Material::FRONT_AND_BACK, osg::Vec4(0.8, 0.8, 0.8, 1.0));
                mat->setDiffuse (osg::Material::FRONT_AND_BACK, osg::Vec4(0.4, 0.4, 0.4, 1.0));
                mat->setSpecular(osg::Material::FRONT_AND_BACK, osg::Vec4(0, 0, 0, 1.0));
                mat->setShininess(osg::Material::FRONT_AND_BACK, 64);
                stateset->setAttribute(mat, osg::StateAttribute::OVERRIDE|osg::StateAttribute::ON);
                // Setup wheel texture
                osg::ref_ptr<osg::Image> image = osgDB::readRefImageFile( "F:/tmp/postgraduate/04bullet/learnCodes/btOsgQt-master/wheel.jpg" );
                if (image) {
                    osg::Texture2D* texture = new osg::Texture2D;
                    texture->setImage(image);
                    texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR);
                    stateset->setTextureAttributeAndModes(0, texture, osg::StateAttribute::ON);
                }
                stateset->setMode(GL_LIGHTING, osg::StateAttribute::OVERRIDE|osg::StateAttribute::ON);
                sd->setStateSet( stateset );
            }   else fprintf(stderr,"Error creating osg::Shape\n");
        } else fprintf(stderr,"Error creating osg::Shape\n");

        osg::PositionAttitudeTransform *gen_wheel = new  osg::PositionAttitudeTransform;
        gen_wheel->setPosition(osg::Vec3(0.,0.,0.));
        gen_wheel->setAttitude(osg::Quat(-osg::PI/2.,osg::Vec3(0.,1.,0.)));
        gen_wheel->addChild(geo);
        printf( "num wheels %d\n",vehicle->getNumWheels());

        // Configures each wheel of our vehicle, setting its friction, damping compression, etc.
        // For more details on what each parameter does, refer to the docs
        for (int i = 0; i < vehicle->getNumWheels(); i++)
        {
            btWheelInfo& iWheel = vehicle->getWheelInfo(i);
            /*
             * iWheel.m_suspensionStiffness = 14.0;
             * iWheel.m_wheelsDampingRelaxation = 0.2;
             * iWheel.m_wheelsDampingCompression = 0.2;
             * iWheel.m_frictionSlip = 1000.;
             * iWheel.m_rollInfluence = 0.01;
             */
            // Affects rest height
            //悬挂刚度，影响下面两个阻尼
            iWheel.m_suspensionStiffness = 15.;
            //压缩阻尼
            iWheel.m_wheelsDampingCompression = 0.3 * 2 * btSqrt(iWheel.m_suspensionStiffness); // 0.8;
            //放松阻尼
            iWheel.m_wheelsDampingRelaxation =  0.5 * 2 * btSqrt(iWheel.m_suspensionStiffness); // 1;

            // iWheel.m_maxSuspensionForce = 150000.; // 6000
            printf("maxSupForce %f\n", iWheel.m_maxSuspensionForce);

            //Larger friction slips will result in better handling
            iWheel.m_frictionSlip = 1.5;
            iWheel.m_rollInfluence = 1;

            wheel[i] = new  osg::PositionAttitudeTransform;
            if ( wheel[i] ) {
                wheel[i]->addChild(gen_wheel);
                //wheel[i]->setPosition(osg::Vec3(bt2osg_Vec3(*halfExtents)));
                /*
                                osg::Vec3 iPos = bt2osg_Vec3(iWheel.m_chassisConnectionPointCS);
                                wheel[i]->setPosition(iPos);
                */
                btosgVec3 iPos = iWheel.m_chassisConnectionPointCS;
                wheel[i]->setPosition(iPos);

                //printf("  Wheel %d, %f %f %f\n",i,iPos[0],iPos[1],iPos[2]);
                model->addChild( wheel[i] );
            }
        }
    }

    void printInfo() {
        /// Outputs vehicle's info.
        btosgPrint("Center of Mass", body->getCenterOfMassPosition());
        btosgPrint("Mass", mass);
        for( int i=0 ; i<vehicle->getNumWheels(); i++)
        {
            btWheelInfo& iWheel = vehicle->getWheelInfo(i);
            btosgPrint("Wheel", i);
            btosgPrint("  ChassisConnectionPoint", iWheel.m_chassisConnectionPointCS);
            btosgPrint("  WheelDirection", iWheel.m_wheelDirectionCS);
            btosgPrint("  WheelAxle", iWheel.m_wheelAxleCS);

        }
    }


    void logPosition() {
        /// Outputs vehicle's position.
        if (body) {
            btTransform wTrans;
            body->getMotionState()->getWorldTransform(wTrans);
            btVector3 pos;
            pos = wTrans.getOrigin();
            if ( name )
                std::cout << "Object   " << name << " position " << pos[0] << " " << pos[1] << " " << pos[2] << std::endl;
            else
                std::cout << "Object _NO_NAME_ position " << pos[0] << " " << pos[1] << " " << pos[2] << std::endl;
            return;
            for( int i=0 ; i<4 ; i++ ) {
                btWheelInfo& iWheel = vehicle->getWheelInfo(i);
                std::cout << "  whell " << i << ", contact: " << iWheel.m_raycastInfo.m_isInContact ;
                printf("  rotation %f\n", iWheel.m_rotation);
            }
        }
    }

    virtual void update()
    {
        /// Vehicle's update callback.
        /// This function is called automatically from World::setpSimulation() for each registered vehicle.
        /// Positions graphical vehicle and wheels from their physhical states.

        // updateVehicle() implemente in Bullet is Not required.
        // Vehicle dynamics are updated in stepSimulation();
        // updateVehicle requires frame_time that may not be available.
        // vehicle->updateVehicle(frame_time);

        // Visual update
        // Standard btosgObject::update() can be used.
        //这让model和坐标和刚体的坐标进行了一个同步
        btosgObject::update();
        //logPosition();

        // Update Wheels
        for (int i = 0; i < vehicle->getNumWheels(); i++)
        {
            // btRaycastVehicle::updateVehicle() calls updateWheelTransform for every wheel.
            // Do not call vehicle->updateWheelTransform() here if btRaycastVehicle::updateVehicle() is called elsewhere.
            //vehicle->updateWheelTransform(i,true);
            btWheelInfo& iWheel = vehicle->getWheelInfo(i);
            if ( wheel[i] ) {
                //chass是底盘的意思
                //m_chassisConnectionPointCS 这个是底盘连接坐标系 chassis 是底盘，connection 连接，connection连接,cs coordinate system 坐标新
                //m_suspensionLength这里获取了车轮的悬挂长度
                //m_wheelDirectionCS获取的车轮子
                btosgVec3 iPos = iWheel.m_chassisConnectionPointCS +
                                 iWheel.m_raycastInfo.m_suspensionLength * iWheel.m_wheelDirectionCS;
                //
                wheel[i]->setPosition(iPos);
                //通过方向向量和旋转角度形成四元组 重点
                btQuaternion steeringOrn(iWheel.m_wheelDirectionCS,-iWheel.m_steering);
                btMatrix3x3 steeringMat(steeringOrn);

                btQuaternion rotatingOrn(iWheel.m_wheelAxleCS,-iWheel.m_rotation);
                btMatrix3x3 rotatingMat(rotatingOrn);

                btMatrix3x3 fullMat = steeringMat*rotatingMat;
                btQuaternion fullQuat;
                fullMat.getRotation(fullQuat);
                wheel[i]->setAttitude(btosgQuat(fullQuat));
            }
        }
    }

};

#endif // BTOSGVEHICLE_H

